A Nonlinear Filter for Range-only Attitude and Position Estimation

A nonlinear filter structure is proposed to estimate the attitude and position of a vehicle using range measurements only. In the setup adopted, the vehicle is equipped with an array of beacons that determine their range to a set of landmarks with known locations. This scenario arises for instance in underwater acoustic navigation and GPS multiple antenna systems. We consider a simple discrete time kinematical model of the vehicle, the state of which can be identified with an element of the Special Euclidean group SE(3). The filter consists of a copy of the kinematic model of the moving body, plus a correction term which is biased towards the Maximum Likelihood (ML) estimate of its position and attitude based on current range measurements. In this sense, the proposed filter belongs to the class of Recursive Maximum Likelihood estimators and, as verified by simulation results, outperforms the static ML estimator even when the vehicle is describing unknown trajectories. In the framework adopted, the estimates evolve naturally on SE(3), thus eliminating the need for a normalization scheme that is recurrent in other formulations in flat Euclidean space. Copyright c ©2004 IFAC.